SuperStrict
Framework BRL.GlMax2D
Import brl.Random
AppTitle = "marching squares + metaballs"
Global resX:Int=800, resY:Int=600
SetGraphicsDriver GLMax2DDriver()
Graphics resX,resY,0,0

SeedRnd(0)'MilliSecs() 

Local numBalls:Int = 10
Local balls:tball[numBalls]
For Local i:Int=0 Until numBalls
	balls[i] = TBall.Create( TPoint.Create( Rand(150,resX-150), Rand(150,resY-150) ), Rand(1,4) )'Rand(1,4)
Next



Local threshold:Float = 0.3 'higher value -> smaller metaballs
Local mbs:tmetaballsystem = TMetaballsystem.Create(balls, threshold) 'balls, gooeyness, and potential field threshold

Local iterations:Int = 0
Local fps:Int = 0
Local lastSec:Long = MilliSecs() + 1000
Local frames:Int = 0

Global bench1:Int = 0
Local closestBall:TBall = Null

While(Not KeyHit(KEY_ESCAPE) And Not AppTerminate()	)
	Cls

	If(MouseDown(1) And closestBall <> Null)
		
		closestBall.pos.x = MouseX()
		closestBall.pos.y = MouseY() 
		
	Else
		'Find ball closest to mouse
		Local minDist:Float = 1e32
		For Local b:TBall = EachIn balls
			Local distSq:Float = (MouseX() - b.pos.x)^2 + (MouseY() - b.pos.y)^2
			If(distSq < minDist)
				closestBall = b
				minDist = distSq
			EndIf
		Next
	EndIf
	
	Local t:Float = MilliSecs() / 1000.0
	
	If MouseDown(2)
		mbs.marchingSquares()
	Else
		mbs.naivePlot()
	EndIf
	
	Local force:Float = mbs.calcForce( TPoint.Create( MouseX(), MouseY() ) )
	SetColor 255,255,255
	DrawText "f:"+force, MouseX(), MouseY()+20

	
	
	If(closestBall <> Null) Then DrawLine(MouseX(), MouseY(), closestBall.pos.x, closestBall.pos.y )
	
	SetColor 255,255,0
	DrawText("iterations: "+iterations+"    fps: "+fps, 0,0)
	'DrawText("bench1: "+(bench1/1000.0),0,15)
	bench1 = 0
	
	If(lastSec < MilliSecs() )
		lastSec = MilliSecs() + 250
		fps = frames*4
		frames = 0
	EndIf
	
	iterations:+1
	frames :+ 1
	Flip
	
Wend
End

Type TPoint
	Field x:Float, y:Float
	
	Function Create:TPoint(x:Float, y:Float)
		Local tmp:TPoint = New TPoint
		tmp.x = x
		tmp.y = y
		Return tmp
	End Function
	
	Method clone:TPoint()
		Local tmp:TPoint = New TPoint
		tmp.x = x
		tmp.y = y
		Return tmp
	End Method
	
	Method set(x:Float, y:Float)
		Self.x = x
		Self.y = y
	End Method
	
	Method add(other:TPoint)
		Self.x :+ other.x; Self.y :+ other.y
	End Method
End Type

Type TLine
	Field x1:Float, y1:Float, x2:Float, y2:Float
	
	Function Create: TLine(x1:Float, y1:Float, x2:Float, y2:Float)
		Local tmp: TLine = New TLine
		tmp.x1 = x1
		tmp.y1 = y1
		tmp.x2 = x2
		tmp.y2 = y2
		Return tmp
	End Function
	
End Type

Type TVector
	Field _x:Float,_y:Float
	
	Method set(x:Float,y:Float)
		Self._x = x
		Self._y = y
	End Method
	
	Method AddVector( v:TVector )
		_x :+ v._x
		_y :+ v._y
	End Method
	
	Method SubtractVector( v:TVector )
		_x :- v._x
		_y :- v._y
	End Method
	
	Method DotProduct:Float( v:TVector )
		Return _x * v._x + _y * v._y
	End Method
	
	Method GetLength:Float( )
		Return Sqr( _x*_x + _y*_y )
	End Method
	
	Method Normalize()
		Local rLen:Float = 1.0 / GetLength()
		_x :* rLen
		_y :* rLen
	End Method
	
	Function Create:TVector( x:Float , y:Float )
		Local t:TVector = New TVector
		t._x = x
		t._y = y
		Return t
	End Function
	
	Function Clone:TVector( v:TVector )
		Local t:TVector = New TVector
		t._x = v._x
		t._y = v._y
		Return t
	End Function
End Type




Type TBall
	
	Field pos:TPoint
	Field pos0:TPoint
	Field edgePos:tpoint
	Field size:Float
	Field tracking:Int 'Bool
	Field trackVectorDir:TVector = New TVector
	
	Field found:Int
	Field isComputed:Int
		
	Function Create:TBall(pos:tpoint, size:Float)
		Local tmp:TBall = New TBall
		tmp.pos = pos
		tmp.pos0 = pos
		tmp.edgePos = pos
		tmp.size = size
		tmp.tracking = False
		Return tmp
	End Function

End Type

Type TMetaballsystem
	Const borderStepSize:Float = 0.01'0.01 'number of pixel to move in search of border offset from center
	Field balls:tball[]
	Field threshold:Float
	
	Field gridSiz:Float = 3'3.0
	Field gridW:Int = resX / gridSiz
	Field gridH:Int = resY / gridSiz
	Field computed:Float[,]
	Field gridBytes:Int
	
	Field vUp:TPoint = TPoint.Create(0, -1)
	Field vDown:TPoint = TPoint.Create(0, 1)		
	Field vLeft:TPoint = TPoint.Create(-1, 0)
	Field vRight:TPoint = TPoint.Create(1, 0)
	
	Field variations_ccw:TPoint[15]
	Field borderList:TList = New TList
	
	Method New()	
		gridBytes = 4*gridW*gridH '4 bytes for float
		'computed = Float Ptr(MemAlloc( gridBytes ))
		computed = New Float[gridW,gridH] 
		
		variations_ccw[%0000] = vRight 'Null
		variations_ccw[%0001] = vDown
		variations_ccw[%0010] = vLeft
		variations_ccw[%0011] = vLeft
		variations_ccw[%0100] = vRight
		variations_ccw[%0101] = VDown
		variations_ccw[%0110] = vLeft
		variations_ccw[%0111] = vLeft
		variations_ccw[%1000] = vUp
		variations_ccw[%1001] = vUp
		variations_ccw[%1010] = vUp
		variations_ccw[%1011] = vUp
		variations_ccw[%1100] = vRight
		variations_ccw[%1101] = vDown
		variations_ccw[%1110] = vRight
		' last is illegal. All pixels filled. No where to go.
	End Method
	
	Function Create:TMetaballsystem(balls:TBall[], threshold:Float)
		Local tmp:TMetaballsystem = New TMetaballsystem
		tmp.balls = balls
		tmp.threshold = threshold
		Return tmp
	End Function 
	
	Method trackBorderOfAllBalls()	
		For Local b1:TBall = EachIn balls
			b1.trackVectorDir.set(0,0)
			For Local b2:TBall = EachIn balls
				If b1 <> b2 
					b1.trackVectorDir._x :+ b2.pos.x - b1.pos.x 'from this to other
					b1.trackVectorDir._y :+ b2.pos.y - b1.pos.y
				EndIf
			Next
			
			b1.trackVectorDir.normalize()
			b1.trackVectorDir._x :* -1
			b1.trackVectorDir._y :* -1
		Next
		
		For Local b:TBall = EachIn balls
			Local centerPlusOne:TPoint = TPoint.Create(b.pos.x, b.pos.y )
			b.pos0 = trackTheBorder2(centerPlusOne, b.trackVectorDir)
			b.edgePos = b.pos0
			b.tracking = True
		Next
	End Method
	
	Method stepOnceTowardsBorder2:tpoint(pos:TPoint,dir:TVector, forceAtPoint:Float) 'Step once towards the border of the metaballs field
		Local stepsize:Float = 0.01
		Return TPoint.Create( pos.x + dir._x*stepsize , pos.y + dir._y*stepsize )
	End Method
		
	Method trackTheBorder2:tpoint(pos:tpoint, dir:TVector)
		Local force:Float = 9e7 '9999999.0
		Local iters:Int = 0;
		While(force > threshold)
			force = calcForce( pos )
			pos = stepOnceTowardsBorder2(pos, dir, force) 
			
			SetColor 255,255,0
			DrawRect pos.x-2, pos.y-2,4,4 'show a little debug output i.e. yellow pixels
			
			iters :+1
			If(iters > 100) 'if it takes to long to find the border/threshold point, break
				Exit
			EndIf
		Wend
		Return pos
	End Method

	Method marchingSquares()
		borderList = New TList
		
		'MemFree computed
		'computed=Float Ptr(MemAlloc(gridBytes))
		'MemClear(computed, gridBytes)
		For Local i%=0 Until gridW
		For Local j%=0 Until gridH
			computed[i,j] = -1
		Next
		Next
		
		'trackBorderOfAllBalls() 'TODO. this isnt accurate enough for marching squares
		
		Local ballNumber:Int = 0
		'Trace the surface of each meta-shape
		For Local b:TBall = EachIn balls
			wikiMarching(b)
			'sakriDotNetMarching()
			
			If Not borderList.isempty()
			SetColor 0,255,0
			
			Local s:Float = gridSiz * .5
			Local old_pos:TPoint = Null
			Local startPos:TPoint = Null
			Local mvx:Float, mvy:Float
			For Local p:TPoint = EachIn borderList
				If 1
					If old_pos = Null
						old_pos = p
						mvx = p.x
						mvy = p.y
						startPos = p
					Else
						Local lx:Float = lerp(old_pos.x, p.x, .5)
						Local ly:Float = lerp(old_pos.y, p.y, .5)
						
						DrawLine(mvx,mvy,lx,ly)
						mvx=lx;
						mvy=ly;
						'Plot lx,ly
						'DrawLine old_pos.x, old_pos.y, p.x, p.y
						old_pos = p
					EndIf
				Else
					DrawRect p.x-s, p.y-s,gridSiz, gridSiz
				EndIf
			Next
			
			If startPos <> Null
				Local lx:Float = lerp(startPos.x, old_pos.x, .5)
				Local ly:Float = lerp(startPos.y, old_pos.y, .5)
				DrawLine mvx,mvy,lx,ly
			EndIf
			
			
			DrawText(""+ballNumber, b.pos.x, b.pos.y )
			ballNumber :+1
			EndIf
		Next
		
		
		'borderList.clear()
	End Method
	
	Method sakriDotNetMarching:TList(ball:TBall)
		'counterclockwise
		
		Local borderList:TList = New TList
		
		Local i:Int = (ball.edgePos.x )/gridSiz
		Local j:Int = (ball.edgePos.y )/gridSiz
		Local first_non_trans:TPoint = TPoint.Create(i,j) 
		'se if this point has been computed/traced earlier. if so, quit.
		
		If calc(i,j) 
			DrawText("skipped", ball.pos.x, ball.pos.y+15 )
			Return borderList
		EndIf
		
		first_non_trans.x :-1; first_non_trans.y :-1 'back up a bit
		Local pos:TPoint = first_non_trans
		borderList.addLast(first_non_trans)
		Local pointNext:TPoint = Null
		Local iters:Int = 0
		Const max_points:Int = 1000
		While(iters<MAX_POINTS)
		
			pointNext = getNextEdgePoint(pos)
			If pointNext.x < 0 Or pointNext.x > (gridW-2) Exit
			If pointNext.y < 0 Or pointNext.y > (gridH-2) Exit
			'If pointNext = first Then Exit 'finished when we go full circle
			If Abs(pointNext.x-first_non_trans.x)<0.01 And Abs(pointNext.y-first_non_trans.y)<0.01 Then Exit
			
			borderList.addLast( TPoint.Create(pointNext.x*gridSiz, pointNext.y*gridSiz) )
			pos = pointNext
			
			iters :+ 1
			If iters>MAX_POINTS Then DebugStop()
			
		Wend
		Return borderList
	End Method
	
	Method getNextEdgePoint:TPoint(pos:TPoint)
		Local index:Int = getMarchVector(pos.x, pos.y) 'getMarchVectorSakri(pos.x, pos.y)
		Local p:TPoint = variations_ccw[index]
		If(p=Null)
			DebugLog "marching squares error. Point not found in march[]" 
			DebugStop()
		EndIf
		
		Return TPoint.Create( pos.x + p.x, pos.y + p.y )
	End Method'

	Method getFirstPoint:TPoint()
		Local firstPoint:TPoint = Null
		For Local y:Int = 1 Until gridH-1 Step 1
		For Local x:Int = 1 Until gridW-1 Step 1
			If calc(x,y)
				firstPoint = TPoint.Create( x,y )
				Return firstPoint
			EndIf
		Next
		Next
		Return Null
	End Method
	
	Method wikiMarching(ball:TBall) 'http://en.wikipedia.org/wiki/Marching_squares	
		
		'Local i:Int = (ball.edgePos.x )/gridSiz
		'Local j:Int = (ball.edgePos.y )/gridSiz
		'se if this point has been computed/traced earlier. if so, quit.
		
		'Local pos:TPoint = TPoint.Create(i,j) 
		Local pos:TPoint = getFirstPoint()
		

		
		If pos = Null Return
		pos.x :-1; pos.y :-1 'back up 1
		
		'If calc( pos.x, pos.y ) 
		'	DrawText("skipped", ball.pos.x, ball.pos.y+15 )
		'	Return
		'EndIf
		
		Local loop:TPoint = Null
		Local index:Int = getMarchVector( pos.x,pos.y )'(gridW-2,gridH-2)
		
		Local march_vector:TPoint = variations_ccw[index]
		
		Local iter:Int = 0
		Local mvx:Float, mvy:Float
		While iter < 1000
			iter:+1
			
			If loop=Null
				loop = pos.clone()
				mvx = pos.x + 1
				mvy = pos.y + 1
			EndIf
			
			pos.x :+ march_vector.x
			pos.y :+ march_vector.y
			
			If(loop)
				borderList.addLast( TPoint.Create(gridSiz*mvx,gridSiz*mvy) ) 
				mvx = pos.x+1; mvy = pos.y+1
			EndIf
			
			Local hitLoop:Int = pos.x-loop.x = 0 And pos.y-loop.y = 0
			If(loop And hitLoop) Exit
			
			If pos.x < 1 Or pos.x > (gridw-1) Exit
			If pos.y < 1 Or pos.y > (gridh-1) Exit
			
			index = getMarchVector(pos.x, pos.y)
			march_vector = variations_ccw[index]
			
		Wend
	End Method
	
	'[g(r) = r^4 - r^2 + 0.25]
	Method calcForce:Float(pos:tpoint) 'called by stepOnceTowardsBorder
		Plot pos.x, pos.y
		Local f:Float = 0.0
		
		'Local rdiv:Float = 1.0/Sqr(resX*resX + resY*resY)
		'Local rdiv:Float = 1e-4
		
		For Local ball:tball = EachIn balls
			Local dx:Float = ball.pos.x-pos.x
			Local dy:Float = ball.pos.y-pos.y
			Local rSquared:Float = (dx*dx+dy*dy)
			'rSquared :*rdiv
			rSquared :/ (1000*ball.size*ball.size)
	
			If rSquared > (0.707*0.707)
			Else
				Local g:Float = rSquared*rSquared* - rSquared + 0.25
				f = f + g
			EndIf
			
		Next
		
		Return Sqr(f) 
	End Method
	
	Method calc:Int(x:Int, y:Int)
		If( computed[x,y] < 0.0001 ) 'has not been calced
			computed[x,y] = calcForce( TPoint.Create(x*gridSiz, y*gridSiz) )
			If computed[x,y] > threshold Return True
		Else
			If computed[x,y] > threshold
				Return True
			Else
				Return False
			EndIf
		EndIf
		
		Return False
	End Method
	
	Method getMarchVector:Int(x:Int, y:Int)

		Local index:Int = %0000
		If( calc(x+1,y+1) ) 
			index = index | %0001
		EndIf
		
		If( calc(x,y+1) )
			index = index | %0010
		EndIf
		
		If( calc(x+1,y) )
			index = index | %0100
		EndIf
		
		If( calc(x,y) )
			index = index | %1000
		EndIf
		
		?debug
		If index = %1111
			DebugLog "Illegal marching squares variation 1111 (all pixels filled/non-transparent). "
			DebugStop()
		EndIf
		?
		
		Return index
	End Method
	
	Method naivePlot() 
		'MemClear( computed, gridBytes)
	
		SetColor 255,255,255
		Local s:Float = 4
		Local test:TPoint = New TPoint
		For Local i:Int = 0 Until gridW Step 5
		For Local j:Int = 0 Until gridH Step 5
			
			test.set(i*gridSiz, j*gridSiz)
			'Local f:Float = calcForce( testPoint )
			'Local idx:Int = Floor(i/gridW)*gridW+ Floor(j/gridH)
			computed[i,j] = calcForce(test)
			
			If computed[i,j] > threshold
				Local c:Int = computed[i,j]*255/0.25
				SetColor c,c,c
				DrawRect test.x-s, test.y-s, s,s
			Else
				SetColor 128,64,0
				DrawRect test.x-s, test.y-s, s,s
			EndIf
		Next
		Next
			
	End Method
	
End Type

Function lerp:Float(x1:Float,x2:Float,t:Float)
	Return x1 + (x2-x1)*t
End Function



